#ifndef ALGORITHM_H
#define ALGORITHM_H

#include <basic.h>

class pidController
{
private:
    float Kp = 0;              // 比例增益
    float Ki = 0;              // 积分增益
    float Kd = 0;              // 微分增益
    float max_output = 0;      // 输出限幅
    float integral = 0;        // 积分累积值
    float prev_error = 0;      // 前一次误差（用于积分和微分）
    float prev_derivative = 0; // 前一次微分值（用于滤波）
    float tau = 0;             // 微分低通滤波时间常数

public:
    pidController(float Kp, float Ki, float Kd, float max_output, float tau = 0.2)
        : Kp(Kp), Ki(Ki), Kd(Kd), max_output(max_output), tau(tau) {};
    void reset(float Kp, float Ki, float Kd, float max_output, float tau = 0.2);
    float update(float error, float dt);
    float update(float ref_val,float act_val, float dt);
};

#endif // ALGORITHM_H